﻿#include<iostream>
#include"my_map.h"
#include "astar.h"
#define ABS(x) ( (x)>0?(x):-(x) )
#define MIN(a, b)  ((a) > (b) ? (b) : (a))
constexpr int DIRX[8] = { 1,1,0,-1,-1,-1,0,1 };  //相邻单元格坐标
constexpr int DIRY[8] = { 0,-1,-1,-1,0,1,1,1 };  //相邻单元格坐标

int startX = 0, startY = 0;  //起始坐标
int endX = 35, endY = 35;  //终点坐标

struct Grid  //方格结构体
{
	int x=0, y=0;  //方格坐标.x为从左往右数第几列,y为从上往下数第几行
	int f=0, g=0, h=0;
	Grid* parent = NULL;  //父节点
	Grid* next = NULL;  //开启列表中的下一个节点
	Grid() {};
	Grid(int _x, int _y) :x(_x), y(_y) {}
};

Grid* OpenList = new Grid(startX, startY);  //开启列表的头指针

//检查输入坐标是否在关闭列表中或是否为终点或不可抵达
int Check_Valid(Grid* grid)
{
	if (test_obstacle(grid->x, grid->y))
		return 1;  //障碍物
	if (test_closelist(grid->x, grid->y))
		return 2;  //在关闭列表中
	if ((grid->x == endX) && (grid->y == endY))
		return 3;  //终点
	return 0;  //坐标有效
}

//检查输入坐标是否在开启列表中
int Check_OpenList(Grid* grid)
{
	Grid* p = OpenList;
	while (p != NULL) {
		if ((p->x == grid->x) && (p->y == grid->y)) {
			grid->g = p->g;
			grid->h = p->h;
			return 4;//在开启列表中
		}
		p = p->next;
	}
	return 0;  //坐标有效
}

int Function_G(int x)
{
	if (x % 2)
		return 14;
	else
		return 10;
}

//目标点与终点的距离
int Block_Distance(int x, int y)
{
	int dx = ABS(x - endX);
	int dy = ABS(y - endY);
	int ans = 14 * MIN(dx, dy) + 10 * ABS(dx - dy);
	return ans;
}

//将输入节点按从小到大顺序插入开启列表
//relocate为0时只插入节点,为1时用于将已在列表中的节点重新调整位置
void Insert_OpenList(Grid* grid, bool relocate)
{
	if (OpenList == NULL) {
		OpenList = grid;  //开启列表为空
		return;
	}
	if (grid->f <= OpenList->f) {  //输入节点插入到列表最前
		grid->next = OpenList;
		OpenList = grid;
		return;
	}
	Grid* p = OpenList;
	bool inserted = 0;  //grid是否已插入新位置
	while (p->next != NULL) {  //不能遍历至p为空,因为可能要在列表最后插入
		if (!inserted && grid->f <= p->next->f) {  //输入节点在列表中间
			grid->next = p->next;
			p->next = grid;
			if (!relocate)
				return;
			else
				inserted = 1;
		}
		p = p->next;
		if (!relocate) continue;
		if (p->next == NULL)
			return;
		if (p->next->x == grid->x && p->next->y == grid->y) {
			Grid* temp = p->next;
			p->next = temp->next;
			delete temp;
			return;
		}
	}
	p->next = grid;//输入节点在列表最后
}

void output(Grid* grid)
{
	while (grid != NULL)
	{
		draw_point(grid->x, grid->y);
		grid = grid->parent;
	}
	mapwrite();
}

int main()
{
	Grid* P;//当前方格
	Grid* AdjoinGrid;  //相邻方格
	int status;  //用于判断方格状态
	int changeG;  //G值的改变
	read_map("map.png");
	while (1) {
		P = OpenList;
		OpenList = OpenList->next;  //将当前方格从开启列表中移出
		add_closelist(P->x, P->y);  //将当前方格移到关闭列表并涂灰色
		mapshow();
		for (int i = 0; i < 8; i++)  //遍历当前方格的8个相邻方格
		{
			AdjoinGrid = new Grid(P->x + DIRX[i], P->y + DIRY[i]);
			status = Check_Valid(AdjoinGrid);
			if (status == 1 || status == 2) {  //当前方格为障碍物或在关闭列表中
				delete AdjoinGrid;
				continue;
			}
			else if (status == 3) {  //终点
				AdjoinGrid->parent = P;
				output(AdjoinGrid);
				return 0;
			}
			status = Check_OpenList(AdjoinGrid);
			if (status == 0) {  //不在开启列表中
				AdjoinGrid->parent = P;  //将这个方格的父对象设为当前方格
				AdjoinGrid->g = P->g + Function_G(i);  //计算该方格的F、G、H值
				AdjoinGrid->h = Block_Distance(AdjoinGrid->x, AdjoinGrid->y);
				AdjoinGrid->f = AdjoinGrid->g + AdjoinGrid->h;
				Insert_OpenList(AdjoinGrid, 0);  //加入开启列表
			}
			else  //在开启列表中
			{
				changeG = P->g + Function_G(i);
				if (changeG < AdjoinGrid->g)  //根据G值检查经由当前方格的路径是否更近
				{
					AdjoinGrid->g = changeG;
					AdjoinGrid->f = AdjoinGrid->g + AdjoinGrid->h;
					AdjoinGrid->parent = P;  //将这个相邻方格的父对象设为当前方格
					Insert_OpenList(AdjoinGrid, 1);  //重新调整这个方格在开启列表中的位置
				}
				else
					delete AdjoinGrid;  //开启列表中已经分配内存,该相邻方格的内存可以释放
			}
		}
	}
	return 0;
}
